#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from zoros.msg import PlannerProperties

class PlannerPropertiesPublisher(Node):
    def __init__(self):
        super().__init__('joint_planner_properties_publisher')
        # 发布器
        self.publisher_ = self.create_publisher(PlannerProperties, '/properties_joint_planner', 10)
        # 定时器，每1秒发送一次
        self.timer = self.create_timer(1.0, self.timer_callback)

    def timer_callback(self):
        msg = PlannerProperties()
        
        # 假设你的机器人是7自由度
        msg.velocities = [2.0] * 7
        msg.accelerations = [5.0] * 7
        msg.jerks = [10.0] * 7

        self.publisher_.publish(msg)
        self.get_logger().info('Published JointPlannerProperties message')

def main(args=None):
    rclpy.init(args=args)
    node = PlannerPropertiesPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
